﻿using ScFramework.Core;
using System;
using System.Collections;
using System.Collections.Generic;
using UnityEngine;


public partial class ConnectManager
{
    void ResetRigidCenter(uint rigidId)
    {
        var es = _es2.FindAll(s => s.AttachedRigidUnion == rigidId);
        var p = GetLocalMassCenter(es);
        Get1(rigidId).LocalPosition = Get1(rigidId).LocalPosition + (Vector3)p;
        foreach(var x in es)
        {
            x.LocalPosition = x.LocalPosition - (Vector3)p;
        }
    }

    void dfs(uint x,uint fa,ref List<uint>list)
    {
        list.Add(x);
        var comps = connectPairs.FindAll(s => s.Comp1 == x || s.Comp2 == x);
        foreach(var c in comps)
        {
            uint nx;
            if (c.Comp1 == x) nx = c.Comp2;
            else nx = c.Comp1;
            if (list.Contains(nx)==false)
            {
                dfs(nx, x,ref list);
            }
        }
    }
    #region break;
    void BreakTwoCollider(uint c1, uint c2)
    {
        connectPairs.RemoveAll(s => s.Equals(new ConnectPair(c1, c2)));
        var list = new List<uint>();
        dfs(c1, 0, ref list);
        if (list.Contains(c2) == true)
        {
            return;
        }
        var list1 = new List<uint>();
        dfs(c2, 0, ref list1);
        var r = Get1(Get2(c1).AttachedRigidUnion);
        Dispatch(EntityDataEvent.Create, new DataCreateArgs
        {
            ID = rigidUnionID,
            actions = (EntityData d) =>
            {
                var nr = new SRigidUnionVEx(d.GetComponentData<SRigidUnion>());

                nr.AngleVelocity = r.AngleVelocity;
                nr.LocalPosition = r.LocalPosition;
                nr.LocalRotation = r.LocalRotation;
                nr.LocalIsFliped = r.LocalIsFliped;
                var core = new SCoreVEx(GameObject.FindObjectOfType<SCore>().SData);
                Debug.Log(GameObject.FindObjectOfType<SCore>().transform.localPosition);
                Debug.Log("p is " + nr.LocalPosition);
                nr.Velocity = r.Velocity;

                foreach(var x in list1)
                {
                    Get2(x).AttachedRigidUnion = nr.CompIdentity;
                }
            }
        });

    }
    #endregion

    #region connect
    void Connect(InterfaceInfo a, InterfaceInfo b)
    {
        if(Get2(a.CompId).AttachedRigidUnion==Get2(b.CompId).AttachedRigidUnion)
        {
            return;
        }
        if (ContainConnect(new ConnectPair (a.CompId,b.CompId )))
        {
            return;
        }
        connectPairs.Add(new ConnectPair
        (
            a.CompId,
            b.CompId
        ));
        var ca = Get2(a.CompId);
        var cb = Get2(b.CompId);
        //获取刚体
        var ra = Get1(ca.AttachedRigidUnion);
        var rb = Get1(cb.AttachedRigidUnion);
        var lista = _es2.FindAll(s => s.AttachedRigidUnion == ra.CompIdentity);
        var listb = _es2.FindAll(s => s.AttachedRigidUnion == rb.CompIdentity);
        //两刚体合并前的质量
        var massa = GetMass(lista);
        var massb = GetMass(listb);
        //合并前的速度和角速度
        Vector3 va = ra.Velocity, vb = rb.Velocity;
        float rva = ra.AngleVelocity, rvb = rb.AngleVelocity;
        //合并的目标接口
        //合并前两刚体的仿射矩阵
        var ma = Matrix4x4.TRS(ra.LocalPosition, Quaternion.Euler(0, 0, ra.LocalRotation),new Vector3(ra.LocalIsFliped?-1:1,1,1));
        var mb = Matrix4x4.TRS(rb.LocalPosition, Quaternion.Euler(0, 0, rb.LocalRotation), new Vector3(rb.LocalIsFliped ? -1 : 1, 1, 1));
        //两接口的世界坐标和世界坐标上的接口朝向
        var ta = GetWorldPositionAndDir(ma, a,ca);
        var tb= GetWorldPositionAndDir(mb, b, cb);
        //接口朝向间的角度
        float ro = (Vector3.Angle(ta.Item2, -tb.Item2));
        //massa/(massa+massb)
        var ratio = 1f / (1f + massb/ massa);

        ra.LocalRotation += ro * (1-ratio);
        var pa =  ma.inverse.MultiplyPoint(ta.Item1);
        var nma= Matrix4x4.TRS(ra.LocalPosition, Quaternion.Euler(0, 0, ra.LocalRotation+ro*(1-ratio)), new Vector3(ra.LocalIsFliped ? -1 : 1, 1, 1));
        pa = nma.MultiplyPoint(pa);

        var pb = mb.inverse.MultiplyPoint(tb.Item1);
        var nmb = Matrix4x4.TRS(rb.LocalPosition, Quaternion.Euler(0, 0, rb.LocalRotation+ro*ratio), new Vector3(rb.LocalIsFliped ? -1 : 1, 1, 1));
        pb = nmb.MultiplyPoint(pb);

        
        var dir = Matrix4x4.Rotate(Quaternion.Euler(0, 0, ro * (1 - ratio))).MultiplyVector(ta.Item2);
        dir.Normalize();
        float dis = Vector2.Distance(pa, pb);
        pa =  (dis - Main.ObejectBlockSize) * dir * (1 - ratio);
        pb= (dis - Main.ObejectBlockSize) * -dir * (ratio);
        nma = Matrix4x4.Translate(pa) * nma;
        nmb = Matrix4x4.Translate(pb) * nmb;


        //      var scale = nmb.lossyScale / nma.lossyScale;
        foreach (var x in listb)
        {
         //   var scale=
            var mat = Matrix4x4.TRS(x.LocalPosition, Quaternion.Euler(0,0,x.LocalRotation),new Vector3(x.LocalIsFliped?-1:1,1,1));

          //  Debug.Log(mat.rotation.eulerAngles+" " + Matrix4x4.TRS(Vector3.zero, Quaternion.Euler(0, 0, 90), new Vector3(-1, 1, 1)).rotation.eulerAngles);

            mat =nma.inverse* nmb * mat;
            x.LocalPosition = mat.MultiplyPoint(Vector3.zero);

         //   Debug.Assert(Mathf.RoundToInt(mat.lossyScale.x) == -1 && Mathf.RoundToInt(mat.lossyScale.y) == 1);
            x.LocalIsFliped = mat.lossyScale.x<0?true:false;
            mat *= Matrix4x4.TRS(Vector3.zero, Quaternion.Euler(0, 0, 0), mat.lossyScale).inverse;
            var dd = mat.MultiplyVector(new Vector3(1, 0, 0));

            x.LocalRotation = Mathf.Atan2(dd.y, dd.x)/Mathf.PI*180f;
            x.AttachedRigidUnion = ra.CompIdentity;
        }



        Dispatch(EntityDataEvent.Delete, rb.CompIdentity.left());

    //    ra.LocalPosition = nma.MultiplyPoint(Vector3.zero);
    //    var ndir = (nma.MultiplyVector(Vector3.right));
    //    ra.LocalRotation = Mathf.Atan2(ndir.y, ndir.x);

        ra.Velocity = (va * massa + vb * massb) / (massa + massb);
        ra.AngleVelocity = (rva * massa + rvb * massb) / (massa + massb);
    }
    Tuple<Vector3, Vector3> GetWorldPositionAndDir(Matrix4x4 rigidMat,InterfaceInfo info,SColliderVEx com)
    {
        Matrix4x4 mat = Matrix4x4.TRS((Vector3)com.LocalPosition, Quaternion.Euler(0, 0, com.LocalRotation),new Vector3 (com.LocalIsFliped?-1:1,1,1));
        var pos = (rigidMat * mat).MultiplyPoint((Vector3)info.LocalMeshPos*Main.ObejectBlockSize);
        var rot = (rigidMat * mat).MultiplyVector((info.InterfacePos*90f).ToDirVector()).normalized;
        return new Tuple<Vector3, Vector3>(pos, rot);
    }
    Vector2 GetLocalMassCenter(List<SColliderVEx> list)
    {
        Vector2 center = Vector2.zero;
        foreach(var x in list)
        {
            center += x.Mass *(Vector2) x.LocalPosition;
        }
        return center / list.Count;
    }
    float GetMass(List<SColliderVEx>list)
    {
        var mass = 0f;
        foreach(var x in list)
        {
            mass += x.Mass;
        }
        return mass;
    }
    #endregion
}